#include "cfdl_ptl_impl.h"

#include <random>
#include <iostream>
#include <stdio.h>

CFDLPtlImpl::CFDLPtlImpl()
{
    std::default_random_engine e;
    _send_seq = e();

    std::cout << "cfdl ptl creator" <<std::endl;
}

CFDLPtlImpl::~CFDLPtlImpl()
{
    for(auto &s : _cmd_map) {
        cmd_module_node  *_node = s.second;
        delete  _node;
    }

    _cmd_map.clear();

    std::cout << "cfdl ptl destory" <<std::endl;
}

void CFDLPtlImpl::register_cmd_module(uint16_t cmd, cfdl_msg_fun_cb cb, cfdl_msg_fun_cb e)
{
    cmd_module_node* _node = new cmd_module_node();

    _node->msgid  = cmd;
    _node->cb_fun = std::move(cb);
    _node->ack_e  = std::move(e);

    _cmd_map[cmd] = _node;
}

void CFDLPtlImpl::cmd_module_exec(cfdl_message_t* msg)
{
    printf("recv cmd:%X\n", msg->msid);
    fflush(stdout);
    if(msg->msid==0) {
        //如果是请求帧
        if(CF_ISSELF(msg->dst)){
            if(msg->type == 0){
                cfdl_ro_send_heartbeat(1, msg->src, FRAME_ACK_TYPE);
            }
        }
    }
    auto it = _cmd_map.find(msg->msid);
    if(it != _cmd_map.end()) {
        it->second->cb_fun(msg);
    }
}

void CFDLPtlImpl::cmd_module_exec_err(cfdl_message_t* msg)
{
    auto it = _cmd_map.find(msg->msid);
    if(it != _cmd_map.end()) {
        it->second->ack_e(msg);
    }
}

void CFDLPtlImpl::ptl_run(uint16_t ms)
{
    _ptl_tick += ms;
    broadcast_heart();
}

int CFDLPtlImpl::ptl_io_in(uint8_t *data, uint16_t len)
{
    return ptl_io_in_parse(data, len);
}

void CFDLPtlImpl::register_send_fun(std::function<void(const char*, uint16_t)> fun_send)
{
    _send_data = std::move(fun_send);
}

int CFDLPtlImpl::ptl_io_out(cfdl_message_t *msg)
{
    uint8_t check;
    int i;
    int sen_len = 0;

    if(msg == nullptr || ((msg->len>0) && (msg->payload==0))) return __LINE__;//发送数据异常

    if(msg->len > CFDL_MSG_LEN) {
        __cfdl_err_(CFDL_WLEN_E,0);
        return __LINE__;
    }

    uint8_t *sen_temp = new uint8_t[sizeof(cfdl_message_t)+msg->len];
    if(sen_temp == 0){
        __cfdl_err_(CFDL_EMA_NO_FREE_MEMORY,msg->msid);
        return __LINE__;
    }
    sen_temp[sen_len] = 0x55;//头
    sen_len++;
    sen_temp[sen_len] = msg->type|(CFDL_BASEVERSION<<4);//帧类型
    sen_len++;
    memcpy(&sen_temp[sen_len],(const void *)&msg->len,14);
    sen_len += 14;
    memcpy(&sen_temp[sen_len],msg->payload,msg->len);
    sen_len += msg->len;
    check=0;
    for(i=0;i<sen_len;i++)check+=sen_temp[i];
    sen_temp[sen_len] = check;
    sen_len++;

    if(_send_data)
        _send_data((char *)sen_temp, sen_len);

    delete []  sen_temp;

    return 0;
}

int CFDLPtlImpl::ptl_io_in_parse(uint8_t* data, int len)
{
    int i,ret = 0;
    uint8_t C;
    cfdl_io_node_t *dv;
    cfdl_decode_msg_t *msg;

    for(i = 0;i < len;i++)
    {
        C=data[i];
        dv  = &io_node_list;
        msg = &dv->read_temp;
        switch (dv->read_state) {
        case 0://监听包头
            if(C == 0x55) {
                msg->cfdl_data[dv->read_state] = C;
                dv->check = 0;
                dv->read_state++;
            }
            break;
        case 1://监听帧类型
            if((C>>4) != CFDL_BASEVERSION) {
                dv->read_state=0;
                return __LINE__;
            }
            msg->cfdl_data[dv->read_state] = C;
            dv->read_state++;
            break;
        case 2://监听长度 bit7:0
        case 3://监听长度 bit15:8
        case 4://监听SEQ bit7:0
        case 5://监听SEQ bit15:8
        case 6://监听源CHID
        case 7://监听源VID
        case 8://监听源PID bit7:0
        case 9://监听源PID bit15:8
        case 10://监听目标CHID
        case 11://监听目标VID
        case 12://监听目标PID bit7:0
        case 13://监听目标PID bit15:8
        case 14://监听消息ID bit7:0
            msg->cfdl_data[dv->read_state] = C;
            dv->read_state++;
            break;
        case 15://监听消息ID bit15:8
            msg->cfdl_data[dv->read_state] = C;
            dv->payload_r=0;
            if(msg->msg.len > CFDL_MSG_LEN) { //包过长
                __cfdl_err_(CFDL_RLEN_E, msg->msg.msid);
                dv->read_state = 0;
                break;
            }
            if(msg->msg.len == 0) {
                dv->read_state+=2;
            }
            else {
                msg->msg.payload = new uint8_t[msg->msg.len];
                dv->read_state++;
            }
            break;
        case 16://监听payload
            msg->msg.payload[dv->payload_r] = C;
            dv->payload_r++;
            if(dv->payload_r>=msg->msg.len) {
                dv->payload_r=0;
                dv->read_state++;
            }
            break;
        case 17://监听check
            msg->msg.check = C;
            //校验和
            if(msg->msg.check == dv->check) {
                msg->msg.type &= 0x0F;
                cmd_module_exec(&msg->msg);
                ret = 0;
            }
            else {
                printf("cfdl check error, msgid:%X, %02X-%02X", msg->msg.msid, msg->msg.check, dv->check);
                if(msg->msg.len > 0) {
                    delete [] msg->msg.payload;
                    msg->msg.payload = nullptr;
                    msg->msg.len = 0;
                }
                ret = __LINE__;
                __cfdl_err_(CFDL_CHECK_E,msg->msg.msid);
            }
            dv->check=0;
            dv->read_state = 0;
            break;
        default:
            dv->check=0;
            dv->read_state = 0;
            break;
        }
        dv->check+=C;
    }

    return ret;
}

void CFDLPtlImpl::__cfdl_err_(int err, uint16_t msgid)
{
    printf("cfdl err:%d, msgid:%X\n", err, msgid);
    fflush(stdout);
}

void CFDLPtlImpl::broadcast_heart()
{
    if((_ptl_tick - _heart_tick) >= 2000){
        _heart_tick = _ptl_tick;

        CFID id;
        id.chid = 0;  id.pid = 0; id.vid = 0;
        cfdl_ro_send_heartbeat(0, id, 0);
    }
}

void CFDLPtlImpl::clr_heart_send()
{
    _heart_tick = _ptl_tick;
}

void CFDLPtlImpl::cfdl_ro_send_heartbeat(uint8_t type,CFID id,uint8_t frame_type)
{
    cfdl_message_t msg;

    heartbeat.Ability = CFDL_MSG_LEN;
    heartbeat.MVersion = CFDL_VERSION_M;
    heartbeat.SVersion = CFDL_VERSION_S;
    heartbeat.QPS = 0; //self_cfdl_qps;
    heartbeat.RunTime = _ptl_tick;
    heartbeat.Deep = 0;
    heartbeat.HBTime = 0x2c;

    memset(&msg,0,sizeof(msg));
    if(type) {
        msg.dst.pid = id.pid;
        msg.dst.vid = id.vid;
        msg.dst.chid = id.chid;
        msg.type = frame_type;
    }
    msg.seq = _send_seq++;
    msg.len = sizeof(heartbeat);
    msg.src.pid=CFDL_PID;
    msg.src.vid=CFDL_VID;
    msg.src.chid = cfdl_chid;
    msg.payload  = (uint8_t*)&heartbeat;

    ptl_io_out(&msg);
}
